import cv2
import numpy as np

roadside_intrinsic = {
    "left": [[4.3952134801844204e+02, 0., 6.1413867707971588e+02], [0.,
4.4151021555894147e+02, 3.6186498159180780e+02],[ 0., 0., 1.]],
    "front": [[431.84774, 0, 602.11618], [0, 429.92219, 379.97142], [0, 0, 1.]],
    "right": [[428.32279, 0, 616.24267, ], [0, 427.12734, 308.74042, ], [0, 0, 1]],
}


# front,left,right 畸变系数
roadside_K = {
    "left": [-6.5219447236821299e-02, -6.8346300066684474e-03,
-2.0836096489826551e-03, -1.2035477033195911e-03,
4.4016469433024157e-03],
    "front": [-0.074904, 0.003470, -0.000096, -0.001490, 0.000000],
    "right": [-0.074714, 0.003073, 0.000435, 0.001495, 0.000000],
}

s = "right"
img = cv2.imread("/data/percetion/test/2023-02-14-16-30-32/camera/"+s+"/000002.png")
cv2.imwrite("left_ori.png", img)
print(img.shape)

mtx = np.array(roadside_intrinsic[s])
dist = np.array(roadside_K[s])
newcameramtx = np.array(roadside_intrinsic[s])
# undistort
mapx,mapy = cv2.initUndistortRectifyMap(mtx,dist,None,newcameramtx,(1280,720),5)
dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR)
cv2.imwrite('left_0.png',dst)


dst = cv2.undistort(img, np.array(roadside_intrinsic[s]), np.array(roadside_K[s]))
cv2.imwrite("left_1.png", dst)



